//--- Projeto Domus
// interactionsource.com
// Koji Pereira, Andre Veloso e Marcos Paulo

/*
 Projeto Domus
 interactionsource.com
 Koji Pereira, Andre Veloso e Marcos Paulo

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 3 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
*/


import processing.opengl.*;
import oscP5.*;

OscP5 oscP5;

float xrot = 0;
float zrot = 0;

float xrot_targ = 0;
float zrot_targ = 0;
float orientation = 0;

int redVal;
int greenVal;
int blueVal;

float valX;
float valY;
float valZ;

float h = 0;
float s = 1.0;
float v = 0.8;

float dampSpeed = 5;

float resultante;

float oldResultante;

float diffResultante;

float varResultante = 0;

float oldValX = 0;
float oldValY = 0;
float oldValZ = 0;

float accelX = 0;
float accelY = 0;
float accelZ = 0;

float accelVector = 0;

float diffX = 0;
float diffY = 0;
float diffZ = 0;

int red_min = 150;
int red_max = 255;

int blue_min = 185;
int blue_max = 255;

int green_min = 195;
int green_max = 255;

float pitch, roll = 0;

PFont fontA;


float  x1, y1, z1 = 0.0F;
float  x2, y2, z2 = 0.0F;
float  x3, y3, z3 = 0.0F;
float  x4, y4, z4 = 0.0F;
float  diffX1, diffX2, diffX3, diffX4 = 0;
float  diffY1, diffY2, diffY3, diffY4 = 0;
float  diffZ1, diffZ2, diffZ3, diffZ4 = 0;

void setup() {
	size(640,480, P3D);
	// smooth();
	oscP5 = new OscP5(this, 6001);

	fontA = loadFont("SansSerif-30.vlw");
	textFont(fontA, 20);

}

void draw() {



	background(255);

	oldResultante = resultante;

	diffResultante = resultante - 35;

	oldValX = valX;
	oldValY = valY;
	oldValZ = valZ;

			//Guarda as variaveis para analise temporal
			x4 = x3;
			x3 = x2;
			x2 = x1;
			x1 = oldValX;

			diffX1 = abs(valX - x1);
			diffX2 = abs(valX - x2);
			diffX3 = abs(valX - x3);
			diffX4 = abs(valX - x4);
			System.out.println("Diff -  diffX1:" + diffX1 + " diffX2:" + diffX2 +
					           " diffX3:" + diffX3 + " diffX4:" + diffX4);


	fill(0);
	text("Forca Res:", 30, 30);
	text(resultante/34, 150, 30);
	text("G:", 220, 30);

	text("VAr Res:", 30, 60);
	text(varResultante/34 , 150, 60);
	text("G:", 220, 60);



	text(accelVector, 500, 30);
	text("Accel Vector Res:", 330, 30);

	stroke(0);
	strokeWeight(50);
	line(30, 300, 30, 301-varResultante );


        stroke(127);
	strokeWeight(30);
	line(500, 400, 500, 401-diffX1 );


        stroke(127);
	strokeWeight(30);
	line(540, 400, 540, 401-diffX2 );


        stroke(127);
	strokeWeight(30);
	line(580, 400, 580, 401-diffX3 );

        stroke(127);
	strokeWeight(30);
	line(620, 400, 620, 401-diffX4 );


	pushMatrix();
	translate(width/2, height/2, 1);

	rotateX(-PI/6*(mouseX*0.01));
	rotateY(-PI/6*(mouseY*0.01));


	fill(0);
	text("+Z:", 0, -100);
	text("-Z:", 0, 100);

	fill(0);
	text("-Y:", -100, 0);
	text("+Y:", 100, 0);

	fill(0);
	text("-X:", 0, 0, -100);
	text("+X:", 0, 0, 100);

	stroke(255,255,0);
	strokeWeight(2);
	line(-600,0,0, 600, 0,0);

	stroke(0,255,255);
	strokeWeight(2);
	line(0,-600,0, 0, 600,0);

	stroke(255,0,255);
	strokeWeight(2);
	line(0,0,-600, 0, 0, 600);


	//print("diffResultante: " + diffResultante);

	noFill();

        fill(redVal, greenVal, blueVal, 127);
	stroke(redVal, greenVal, blueVal);

	if ( (diffResultante > 60) &&  (accelVector>80)  ) {
		//println(" - ######## subindo  ");
		fill(255,0,0);
		stroke(255,0,0);
	} else if (( diffResultante < 60 ) && ( diffResultante >  -30) ) {
		//println(" - ### lento ");
		color_transition(varResultante);

	} else if ( (diffResultante < -20) ) {
		//println(" - ############## caindo ");
		fill(0,0,255);
		stroke(0,0,255);
	}

	strokeWeight(2);
	box(70,10,40);


	//Vetor resultante
	stroke(0);
	strokeWeight(10);
	line(0,0,0, valY*1.5, valZ*1.5, valX*1.5);


	//-------------------------------------
	/*
  //Vetor forca individual
  stroke(255,255,0);
  strokeWeight(5);
  line(0,0,0, -valY*1.5, 0,0);


  //Vetor forca individual
  stroke(0,255,255);
  strokeWeight(5);
  line(0,0,0, 0, -valZ*1.5,0);



  //Vetor forca individual
  stroke(255,0,255);
  strokeWeight(5);
  line(0,0,0, 0, 0, -valX*1.5);
	 */

	//------------------------------
	/*
  //Vetor forca individual
  stroke(255,255,0);
  strokeWeight(5);
  line(0,0,0, -accelY*1.5, 0,0);


  //Vetor forca individual
  stroke(0,255,255);
  strokeWeight(5);
  line(0,0,0, 0, -accelZ*1.5,0);


  //Vetor forca individual
  stroke(255,0,255);
  strokeWeight(5);
  line(0,0,0, 0, 0, -accelX*1.5);
	 */

	//----------------------------------------
	//Vetor accel resultante
	stroke(255,0,0);
	strokeWeight(10);
	line(0,0,0, accelY*1.5, accelZ*1.5, accelX*1.5);


	//translate(-width/2, -height/2, -1);


	popMatrix();

	//Pitch - Rotation around the lateral (X) axis
	pitch = atan2(valY, sqrt( sqrt(valX) + sqrt(valZ) )) * -1;

	//Roll  - Rotation around the longitudinal (Y) axis
	roll = atan2(valX, sqrt( sqrt(valY) + sqrt(valZ) )) * -1;


	fill(0);
	text("accelX:", 30, 400);
	text(accelX, 150, 400);

	text("accelY:", 30, 425);
	text(accelY, 150, 425);

	text("accelZ:", 30, 450);
	text(accelZ, 150, 450);

	/*
  text("Pitch:", 320, 400);
  text(pitch, 400, 400);

  text("Roll:", 320, 450);
  text(roll, 400, 450);
	 */




}

void oscEvent(OscMessage theOscMessage) {


	String addr = theOscMessage.addrPattern();
	//println(addr);   // uncomment for seeing the raw message
	//float  val  = theOscMessage.get(0).floatValue();




	if(addr.indexOf("/wiimotePD/gX") !=-1){ // one of the faders
		valX  = Float.valueOf( theOscMessage.get(0).intValue() );
		//valX /= 100;

	}

	if(addr.indexOf("/wiimotePD/gY") !=-1){ // one of the faders
		valY = Float.valueOf( theOscMessage.get(0).intValue() );
		//valY /= 100;

	}

	if(addr.indexOf("/wiimotePD/gZ") !=-1){ // one of the faders
		valZ  = Float.valueOf( theOscMessage.get(0).intValue() );
		//valZ  /= 100;
	}






	resultante = sqrt( sq(valX) + sq(valY) + sq(valZ) );

	varResultante = (resultante-oldResultante) ;

	if (varResultante<0)
		varResultante *= -1;

	accelVector = sqrt( sq(accelX) + sq(accelY) + sq(accelZ) );


	accelX = valX - oldValX;
	accelY = valY - oldValY;
	accelZ = valZ - oldValZ;


}

// This function updates one of the color variables
// either getting brighter or getting dimmer.
// It also updates the outputs and delaYs for 10 milliseconds.
void color_transition( float qtMov )
{
     
	qtMov = constrain(qtMov, 0, 100.0);

	h = h + map(qtMov, 0, 100, 0, 9);

	if (h > 360.0)  h -= 360.0;

	hsv_to_rgb(h, s, v);
	//update();

}

void hsv_to_rgb(float h, float s, float v) {

	int h_i = ((int)(h/60)) % 6;

	float f = (h/60) - (int)(h/60);

	float r = 0;
	float g = 0;
	float b = 0;

	float p = v * (1.0 - s);
	float q = v * (1.0 - f*s);
	float t = (1.0 - (1.0 - f)*s);

	switch(h_i) {
	case 0:  r = v; g = t; b = p; break;
	case 1:  r = q; g = v; b = p; break;
	case 2:  r = p; g = v; b = t; break;
	case 3:  r = p; g = q; b = v; break;
	case 4:  r = t; g = p; b = v; break;
	case 5:  r = v; g = p; b = q; break;
	}

	redVal = red_max - (int)((red_max - red_min)*r);
	greenVal = green_max - (int)((green_max - green_min)*g);
	blueVal = blue_max - (int)((blue_max - blue_min)*b);
}

void update() {
	//analogWrite(LedR, redVal);
	//analogWrite(LedG, greenVal);
	//analogWrite(LedB, blueVal);

}

